/*
 * BalancinFisica.cpp
 *
 *  Created on: Oct 8, 2013
 *      Author: Hernan
 */

#include "BalancinFisica.h"
#include "SogaFisica.h"

BalancinFisica::BalancinFisica(b2World* world, float x, float y, float ancho, float alto, float angulo, string tipo) : EntidadFisica(tipo) {
	this->tipo = "balancin";
	//hay que crear un cuadradito fijo para q el balancin rote sobre el
	b2BodyDef* defBodyCuadradito = new b2BodyDef();
	//defBodyCuadradito->type = b2_kinematicBody;
	defBodyCuadradito->position.Set(x, y);
	this->cuadradito = world->CreateBody(defBodyCuadradito);

	b2PolygonShape formaCuadradito;
	formaCuadradito.SetAsBox(1.0f, 1.0f);
	b2FixtureDef* fixtureCuadradito = new b2FixtureDef();
	fixtureCuadradito->density = 0.0f;
	fixtureCuadradito->shape = &formaCuadradito;
	fixtureCuadradito->friction = 0.6f;
	cuadradito->CreateFixture(fixtureCuadradito);

	//balancin
	this->definicionCuerpoFisico->position.Set(x, y);
	this->definicionCuerpoFisico->angle = angulo;
	this->definicionCuerpoFisico->type = b2_dynamicBody;
	this->cuerpoFisico = world->CreateBody(definicionCuerpoFisico);

	b2PolygonShape box;
	box.SetAsBox(ancho/2, alto/2);

	this->fixture = new b2FixtureDef();
	this->fixture->shape = &box;
	this->fixture->density = 4.3f;

	this->cuerpoFisico->CreateFixture(this->fixture);

	b2RevoluteJointDef jd;

	//como puedo setear el angulo en la edicion, tengo que encontrar la rotacion relativa:

	float anguloDg = 360 - angulo*180/b2_pi;
	float min, max;

	if (anguloDg < 45){
		max = 45 - anguloDg;
		min = anguloDg + 45;
	}
	if (anguloDg >= 315 && anguloDg < 360){
		min =  anguloDg - 315;
		max = 360 - anguloDg + 45;
	}

	cout<<"max: "<<max<<endl;
	cout<<"min: "<<min<<endl;

	jd.Initialize(this->cuerpoFisico,cuadradito,  cuadradito->GetWorldCenter());//this->cuerpoFisico->GetPosition());
	jd.lowerAngle = -min * b2_pi / 180.0f;//-58.0f * PI / 180.0f;
	jd.upperAngle = max * b2_pi / 180.0f; //58.0f * PI / 180.0f;
	jd.enableLimit = true;
	//jd.motorSpeed = 0.0f;
	//jd.maxMotorTorque = 50.0f;
	//jd.enableMotor = true;
	//jd.collideConnected = false;

	this->joint = (b2RevoluteJoint*) world->CreateJoint(&jd);

	//lo seteo sin fuerzas que lo lleven para arriba:

	this->sogaDerecha = false;
	this->sogaIzquierda = false;

	b2BodyDef* defderecho = new b2BodyDef();

	// CONVIERTO ANGULO PARA LA SOGA DERECHA
	double anguloRot = this->cuerpoFisico->GetAngle();

	cout << "Angulo Derecho: " << anguloRot*180/b2_pi << endl;


	double xTrasladado = 15;
	double yTrasladado = 0;

	cout << "x: "  << x << " y: " << y << endl;

	cout << "xTrasladado: " << xTrasladado <<
			" yTrasladado: " << yTrasladado << endl;

	double xRotado = xTrasladado * cos(anguloRot) -
			yTrasladado	* sin(anguloRot);
	double yRotado = xTrasladado * sin(anguloRot)	+
			yTrasladado	* cos(anguloRot);

	cout << "xRotado: " << xRotado << " yRotado: " << yRotado << endl;

	double xAObtener = xRotado + x;
	double yAObtener = yRotado + y;

	cout << "xAObtener: " << xAObtener <<
			"yAObtener: " << yAObtener << endl;

	// FIN CONVERSION PARA LA SOGA DERECHA
	//defderecho->position.Set(x+15, y);
	defderecho->position.Set(xAObtener, yAObtener);
	defderecho->type = b2_dynamicBody;
	this->extremoder = world->CreateBody(defderecho);

	b2PolygonShape caja;
	caja.SetAsBox(0.5f, 0.5f);

	this->extremoder->CreateFixture(&caja,0.0f);

	b2BodyDef* defizquierdo = new b2BodyDef();
	defizquierdo->type = b2_dynamicBody;

	// CONVIERTO ANGULO PARA LA SOGA IZQUIERDA
	float anguloRotIzq = this->cuerpoFisico->GetAngle() + b2_pi;

	cout << "Angulo IZQ: " << anguloRotIzq*180/b2_pi << endl;


	xTrasladado = 15;
	yTrasladado = 0;

	cout << "x: "  << x << " y: " << y << endl;

	cout << "xTrasladado: " << xTrasladado <<
			" yTrasladado: " << yTrasladado << endl;

	xRotado = xTrasladado * cos(anguloRotIzq) -
			yTrasladado	* sin(anguloRotIzq);
	yRotado = xTrasladado * sin(anguloRotIzq)	+
			yTrasladado	* cos(anguloRotIzq);

	cout << "xRotado: " << xRotado << " yRotado: " << yRotado << endl;

	xAObtener = xRotado + x;
	yAObtener = yRotado + y;

	cout << "xAObtener: " << xAObtener <<
			"yAObtener: " << yAObtener << endl;

	// FIN CONVERSION PARA LA SOGA IZQUIERDA

	//defizquierdo->position.Set(x-15, y);
	defizquierdo->position.Set(xAObtener, yAObtener);
	this->extremoizq =  world->CreateBody(defizquierdo);

	this->extremoizq->CreateFixture(&caja,0.0f);

	b2RevoluteJointDef* jointDerDef = new b2RevoluteJointDef();
	jointDerDef->Initialize(this->cuerpoFisico,extremoder,  extremoder->GetPosition());
	world->CreateJoint(jointDerDef);

	b2RevoluteJointDef* jointDerDef2 = new b2RevoluteJointDef();
	jointDerDef2->Initialize(this->cuerpoFisico,extremoizq,  extremoizq->GetPosition());
	world->CreateJoint(jointDerDef2);


	this->guardarInformacionDeEntidad();
	this->alto = alto;
	this->ancho = ancho;

}

string BalancinFisica::getTipo(){
	return this->tipo;
}

void BalancinFisica::aplicarFuerzasFisicas() {

}


void BalancinFisica::tieneSogaDerecha(bool valor){
	this->sogaDerecha = valor;
}

void BalancinFisica::tieneSogaIzquierda(bool valor){
	this->sogaIzquierda = valor;
}

b2Body* BalancinFisica::getExtremoDerecho(){
	return this->extremoder;
}

b2Body* BalancinFisica::getExtremoIzquierdo(){
	return this->extremoizq;
}

BalancinFisica::~BalancinFisica() {
	// TODO Auto-generated destructor stub
}

